/*
 *  Filename: mpu6050.c
 *  Function: Sensor MPU6050
 *  Created: 2017-06-14
 *  Version: 1.0
 *  Author: Bin
 */

/*-------- Includes --------*/
#include <math.h>
#include "stm32f10x.h"

#include "i2c.h"
#include "mpu6050.h"

/*-------- Function prototypes --------*/
static void MPU6050_Write(uint8_t regAddr, uint8_t data);
static void MPU6050_Read(uint8_t regAddr, uint8_t *pdata);

/*-------- Private functions --------*/
void MPU6050_Write(uint8_t regAddr, uint8_t data)
{
    I2C_Start();
    I2C_WriteByte(MPU6050_ADDR);
    if (I2C_WaitAck())
        while (1);
    I2C_WriteByte(regAddr);
    if (I2C_WaitAck())
        while (1);
    I2C_WriteByte(data);
    if (I2C_WaitAck())
        while (1);
    I2C_Stop();
}

void MPU6050_Read(uint8_t regAddr, uint8_t *pdata)
{
    I2C_Start();
    I2C_WriteByte(MPU6050_ADDR);
    if (I2C_WaitAck())
        while (1);
    I2C_WriteByte(regAddr);
    if (I2C_WaitAck())
        while (1);

    I2C_Start();
    I2C_WriteByte(MPU6050_ADDR + 1);
    if (I2C_WaitAck())
        while (1);
    *pdata = I2C_ReadByte(0);
    I2C_Stop();
}

/*-------- Exported functions --------*/
void MPU6050_Init(void)
{
    // Intialize I2C
    I2C_Init();

    // Intialize MPU6050
    MPU6050_Write(PWR_MGMT_1, 0x00);    // Wake up MPU6050
    MPU6050_Write(SMPLRT_DIV, 0x07);    // 125Hz
    MPU6050_Write(CONFIG, 0x06);        // 5Hz
    MPU6050_Write(GYRO_CONFIG, 0x18);   // GYR = 2000 * data / 32768
    MPU6050_Write(ACCEL_CONFIG, 0x01);  // ACC = 2g * data / 32768
}

void MPU6050_Update(MPU6050_t *p)
{
    int16_t tmp;
    float XZ, YZ;

    // get Acceleration
    MPU6050_Read(ACCEL_XOUT_H, (uint8_t *)&tmp + 1);
    MPU6050_Read(ACCEL_XOUT_L, (uint8_t *)&tmp);
    p->ACC_X = (float)tmp / 16384.0f;

    MPU6050_Read(ACCEL_YOUT_H, (uint8_t *)&tmp + 1);
    MPU6050_Read(ACCEL_YOUT_L, (uint8_t *)&tmp);
    p->ACC_Y = (float)tmp / 16384.0f;

    MPU6050_Read(ACCEL_ZOUT_H, (uint8_t *)&tmp + 1);
    MPU6050_Read(ACCEL_ZOUT_L, (uint8_t *)&tmp);
    p->ACC_Z = (float)tmp / 16384.0f;

    // get angular velocity
    MPU6050_Read(GYRO_XOUT_H, (uint8_t *)&tmp + 1);
    MPU6050_Read(GYRO_XOUT_L, (uint8_t *)&tmp);
    p->GYR_X = (float)tmp / 16.384f;

    MPU6050_Read(GYRO_YOUT_H, (uint8_t *)&tmp + 1);
    MPU6050_Read(GYRO_YOUT_L, (uint8_t *)&tmp);
    p->GYR_Y = (float)tmp / 16.384f;

    MPU6050_Read(GYRO_ZOUT_H, (uint8_t *)&tmp + 1);
    MPU6050_Read(GYRO_ZOUT_L, (uint8_t *)&tmp);
    p->GYR_Z = (float)tmp / 16.384f;

    // get Roll angle and Pitch angle
    XZ = sqrt((p->ACC_X) * (p->ACC_X) + (p->ACC_Z) * (p->ACC_Z));
    YZ = sqrt((p->ACC_Y) * (p->ACC_Y) + (p->ACC_Z) * (p->ACC_Z));
    p->Roll = atan(p->ACC_Y / XZ) / MPU6050_PI * 180.0;
    p->Pitch = atan(p->ACC_X / YZ) / MPU6050_PI * 180.0;
}
